import pyrealsense2.pyrealsense2 as rs
import numpy as np
import cv2

# 创建配置管理器
pipeline = rs.pipeline()
config = rs.config()

# 配置管理器添加深度和RGB流
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# 启动管道
pipeline.start(config)

try:
    while True:
        # 获取帧数据
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()

        if not depth_frame or not color_frame:
            continue

        # 转换图像为NumPy数组
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

        # 对齐图像
        aligned_depth_frame = rs.align(rs.stream.color).process(depth_frame)
        aligned_depth_image = np.asanyarray(aligned_depth_frame.get_data())

        # 展示图像
        cv2.imshow('Aligned Depth', aligned_depth_image)
        cv2.imshow('Color', color_image)

        # 按 'q' 退出循环
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

finally:
    # 关闭管道
    pipeline.stop()
    cv2.destroyAllWindows()